#include <chrono>
#include <memory>
#include <string>
#include <stdlib.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

//#include "tutorial_interfaces/msg/num.hpp"     // CHANGE
#include "wheel_msg/msg/wheelout.hpp"
#include "wheel_msg/msg/wheelin.hpp"
#include "yaokong/yaokong.hpp"

using namespace std::chrono_literals;
#define LUN_R_F 2
#define LUN_R_B 3
#define LUN_L_F 1
#define LUN_L_B 0
#define LINE_MAX_SPEED 4096.0  //直行最大速度
#define CIRCLE_MAX_SPEED 2000.0 //拐弯最大速度
class YaoKong : public rclcpp::Node
{
public:
  YaoKong()
  : Node("YaoKong"), count_(0)
  {
    publisher_ = this->create_publisher<wheel_msg::msg::Wheelin>("yaokong", 10);    // CHANGE
    timer_ = this->create_wall_timer(
      10ms, std::bind(&YaoKong::timer_callback, this));
     subscription_ = this->create_subscription<wheel_msg::msg::Wheelout>(          // CHANGE
      "eight_wheel_info", 10, std::bind(&YaoKong::topic_callback, this, std::placeholders::_1));
    for(int i=0;i<10;i++)
      {
        wz_v[i]=0;
       
      }
    for(int i=0;i<4;i++)   angle_shift[i]=0.0;
  }

private:
  void topic_callback(const wheel_msg::msg::Wheelout::SharedPtr msg) //const       // CHANGE
  {
    //获取摇杆命令，xy轴速度以及z转速，来自于订阅serial_all里节点         
    // vy = msg->yaogan[0];
    vx = msg->yaogan[1];
    wz_new = msg->yaogan[2];
    //限制拐弯速度
    if(vx!=0)
      {
       if((wz_new<20)&&(wz_new>-20))
            { if(max_speed < LINE_MAX_SPEED) max_speed +=10; if(max_speed > LINE_MAX_SPEED)max_speed > LINE_MAX_SPEED;}
         else 
            { if(max_speed > CIRCLE_MAX_SPEED) max_speed -=10; if(max_speed < CIRCLE_MAX_SPEED)max_speed > CIRCLE_MAX_SPEED;}
         
      }
    angle_n[0] = double(msg->anglecur[0] ) * (-180.f / 16384.f); 
    angle_n[1] = double(msg->anglecur[1] ) * (-180.f / 16384.f); 
    angle_n[2] = double(msg->anglecur[2] ) * (-180.f / 16384.f); 
    angle_n[3] = double(msg->anglecur[3] ) * (-180.f / 16384.f); 
     //查看每个轮的速度
    for(int i=0;i<4;i++)
      speed_lun_cur[i*2]=-msg->speedcur[i*2];
    for(int i=0;i<4;i++)
      speed_lun_cur[i*2+1]=msg->speedcur[i*2+1];
    //  RCLCPP_INFO(this->get_logger(),"lun1=%d lun2=%d lun3=%d lun4=%d " , 
    //           speed_lun_cur[0],speed_lun_cur[1],speed_lun_cur[2],speed_lun_cur[3]);
    //  RCLCPP_INFO(this->get_logger(),"lun5=%d lun6=%d lun7=%d lun8=%d " , 
    //           speed_lun_cur[4],speed_lun_cur[5],speed_lun_cur[6],speed_lun_cur[7]);        
    /////////////////
    // for(int i=0;i<4;i++)
    //   speed_mode_cur[i] = (speed_lun_cur[i*2]+speed_lun_cur[i*2+1])/2;
    //  RCLCPP_INFO(this->get_logger(),"lun1=%d lun2=%d lun3=%d lun4=%d " , 
    //            speed_mode_cur[0],speed_mode_cur[1],speed_mode_cur[2],speed_mode_cur[3]);

    //查看每个轮组速度

    ////////////////// 
    // if((vy!=0) || (wz!=0))
    // {
    //   RCLCPP_INFO(this->get_logger(), "vy=%d   wz=%d", msg->yaogan[1],msg->yaogan[2]);  
    // }
     RCLCPP_INFO(this->get_logger(), "anglen0=%f  anglen1=%f", angle_n[0] ,angle_n[1] );
     RCLCPP_INFO(this->get_logger(), "anglen2=%f  anglen3=%f", angle_n[2] ,angle_n[3] );
  }
    
  void timer_callback()
  {
    auto message = wheel_msg::msg::Wheelin();                               // CHANGE
   // angle_nmessage.num = this->count_++;                                        // CHANGE
   // RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", message.num);    // CHANGE
    int cccp,status_temp;
      ///避免过快的操作左右摇杆。
     wz_v_count++;
    if(wz_v_count==10)wz_v_count=0;
    wz_v[wz_v_count]= wz_new;
    int wz_ver=0;
    for(int i=0;i<10;i++)wz_ver+=wz_v[i];
    wz=wz_ver/10;
    if((vx!=0)&(active_status==0))status_temp=0;
    if((vx==0)&(wz!=0))status_temp=1; 

   //驱动算法，得四个速度与四个角度
    Driver_cal();
     cccp=Driver_tiao();    //角度过调整
    
    //  if(cccp!=0)
    //  RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", cccp);
    //装载速度与角度 
    if(active_status==status_temp)
    {
        message.speedset[0] = rpm_b[0] * ( max_speed / 128.f);    //                    
        message.speedset[1] = rpm_b[1] * ( max_speed / 128.f);    //
        message.speedset[2] = rpm_b[2] * ( max_speed / 128.f);    //
        message.speedset[3] = rpm_b[3] * ( max_speed / 128.f);    //
    }
    else
      {
         message.speedset[0] = 0;   //                    
        message.speedset[1] = 0;    //
        message.speedset[2] = 0;    //
        message.speedset[3] = 0;    //
        atcive_count++;
        if(atcive_count>180)
        {
          atcive_count=0; active_status=status_temp;
        }
      }
    message.angleset[0] = angle_b[0] * ( 16384.f / 180.f );
    message.angleset[1] = angle_b[1] * ( 16384.f / 180.f );
    message.angleset[2] = angle_b[2] * ( 16384.f / 180.f );
    message.angleset[3] = angle_b[3] * ( 16384.f / 180.f );

    // message.angleset[0] = 60 * ( 16384.f / 180.f );  //左后轮 
    // message.angleset[1] = -60 * ( 16384.f / 180.f );  //左前轮
    // message.angleset[2] = 30 * ( 16384.f / 180.f ); //右前轮 
    // message.angleset[3] = -30 * ( 16384.f / 180.f ); //右后轮  

    message.motorother[0] = 1;    

    watch[0] = angle_n[0];
    watch[1] = angle_n[1];
     watch[2] = angle_n[2];
    watch[3] = angle_n[3];

    watch[4] = angle_b[0];
    watch[5] = angle_b[1];
    watch[6] = angle_b[2];
    watch[7] = angle_b[3];
    // watch[6] = rpm_b[2];
    // watch[7] = rpm_b[2];

    // watch[6] = rpm_c[1];
    // watch[7] = rpm_c[2];

    // watch[8] = rpm_b[1];
    // watch[9] = rpm_b[2];

    RCLCPP_INFO(this->get_logger(), "angle_curLB=%f ,angle_curLF=%f ,angle_RF=%f, angle_curRB=%f ",
             watch[0],watch[1],watch[2],watch[3]);
    RCLCPP_INFO(this->get_logger(), "angle_setLB=%f ,angle_setLF=%f ,angle_setRF=%f, angle_setRB=%f ",
             watch[4],watch[5],watch[6],watch[7]);         
    RCLCPP_INFO(this->get_logger(), "wz =%d ",wz_new);
     publisher_->publish(message);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<wheel_msg::msg::Wheelin>::SharedPtr publisher_;         // CHANGE
  rclcpp::Subscription<wheel_msg::msg::Wheelout>::SharedPtr subscription_;       // CHANGE
  size_t count_;

  // int16_t speed_set[8];

  double WHEEL_RADIUS = 0.106;
  double WHEEL_BASE =0.860 ;          //0.860;
  double TRACK =0.690;               //   0.690;

  double RY = WHEEL_BASE / 2.f;
  double RX = TRACK / 2.f;

  int vx,vy = 0, wz=0;                                    //接收来自遥控的命令
  int wz_v[10],wz_v_count,wz_new;
  double angle_c[4];                                    //计算结果-角度
  double angle_b[4];                                    //最优结果-角度

  double angle_n[4];                                    //当前结果-角度

  double rpm_c[4];                                      //计算结果-速度
  double rpm_b[4];                                      //最优结果-速度

  double watch[16];
  int16_t speed_mode_cur[4];
  int16_t speed_lun_cur[8];
  int active_status=0,atcive_count=0;
  void Driver_cal(void);
  int Driver_tiao(void);
  double max_speed=4096.0f;
  double angle_shift[4];
  };
int YaoKong::Driver_tiao(void)
{
  int tiao_c=0;
  if(wz==0)return tiao_c;
  double x,max;
  int nob;
  max=0;
   if(vx!=0) //左右拐弯
   {
      for(int i=0;i<4;i++)
      {
      x=angle_n[i]-angle_b[i];
      if(max<abs(x)){max=x; nob=i;}
      }
      x=angle_n[nob]-angle_b[nob];
      angle_b[nob]=angle_b[nob]-x*0.36;
   }
//    #define LUN_R_F 2
// #define LUN_R_B 3
// #define LUN_L_F 1
// #define LUN_L_B 0
   else if(wz!=0) //原地转
    {
      if(wz<0)
        {
        angle_b[1]=angle_b[LUN_L_F]-6.5;
        angle_b[3]=angle_b[LUN_R_B]-6.5;
          angle_b[0]=angle_b[LUN_L_B]-3.5;
        angle_b[2]=angle_b[LUN_R_F]-3.5;
        }
        if(wz>0)
        {
        angle_b[1]=angle_b[LUN_L_F]+6.0;      //15.0;
        angle_b[3]=angle_b[LUN_R_B]+6.0;
        angle_b[0]=angle_b[LUN_L_B]+0.5;
        angle_b[2]=angle_b[LUN_R_F]+6.5;
        }
      for(int i=0;i<4;i++)
      {
          // if(angle_b[i]>0)angle_b[i]-=3.0;
          // if(angle_b[i]<0)angle_b[i]+=3.0;
             if(angle_n[i]<angle_b[i])
                {
                   if(angle_shift[i]<0.0)angle_shift[i]=0.0;
                  angle_shift[i]+=0.08;
                  angle_b[i]=angle_b[i]+angle_shift[i];
                }
             else 
              {
                  if(angle_shift[i]>0.0)angle_shift[i]=0.0;
                  angle_shift[i]+=-0.08;
                  angle_b[i]=angle_b[i]+angle_shift[i]; 
              }   
       
       }
      
    }
    else if(wz==0)for(int i=0;i<4;i++)   angle_shift[i]=0.0;
  return tiao_c;
}
void YaoKong::Driver_cal(void)
{
  int nov_flag;
  //驱动算法矩阵B=AX，计算结果为矢量                 
  double vfr_x = vx + RX * wz;
  double vfr_y = vy + RY * wz;
  double vfl_x = vx - RX * wz;
  double vfl_y = vy + RY * wz;
  double vrl_x = vx - RX * wz;
  double vrl_y = vy - RY * wz;
  double vrr_x = vx + RX * wz;
  double vrr_y = vy - RY * wz;

  //矢量分解，得到速度值和角度                    
  angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  angle_c[2] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  angle_c[0] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  angle_c[3] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  rpm_c[1] = calcDistance(vfl_x, vfl_y);
  rpm_c[2] = calcDistance(vfr_x, vfr_y);
  rpm_c[0] = calcDistance(vrl_x, vrl_y);
  rpm_c[3] = calcDistance(vrr_x, vrr_y);

  //最合适执行的角度与速度
  for( int i = 0; i < 4; i++ )
  {
    //速度上限
    if( rpm_c[i] >= 128 )
    {
      rpm_c[i] = 128;
    }

    //判断是否需要反转
    if( ( angle_c[i] >= 90.0 ) || ( angle_c[i] <= -90.0 ) )
    {
      nov_flag = 1;
    }
    else
    {
      nov_flag = 0;
    }

    //得到最合适角度与速度
    if( nov_flag == 1 )
    {
      angle_b[i] = angle_c[i] - ( sgn(angle_c[i]) * 180.0 );
      rpm_b[i]   = -rpm_c[i];
    }
    else
    {
      angle_b[i] = angle_c[i];
      rpm_b[i]   = rpm_c[i];
    }

  }
  

}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<YaoKong>());
  rclcpp::shutdown();
  return 0;
}